{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import cv2\n",
    "import open3d as o3d\n",
    "import time\n",
    "\n",
    "# 读取深度图像\n",
    "depth_img = cv2.imread(\"../img/1_depth.png\", cv2.IMREAD_GRAYSCALE)\n",
    "\n",
    "\n",
    "def depth_to_point_cloud(depth_img, k):\n",
    "    # 获取深度图像的宽度和高度\n",
    "    h, w = depth_img.shape\n",
    "\n",
    "    # 初始化点云数组\n",
    "    point_cloud = np.zeros((h * w, 3), dtype=np.float32)\n",
    "\n",
    "    # 获取相机内参的逆矩阵\n",
    "    k_inv = np.linalg.inv(k)\n",
    "\n",
    "    for i in range(h):\n",
    "        for j in range(w):\n",
    "            # 如果像素深度值为0，则跳过该点\n",
    "            if depth_img[i, j] == 0:\n",
    "                continue\n",
    "\n",
    "            # 使用内参矩阵的逆计算像素点的3D坐标\n",
    "            p = np.dot(k_inv, np.array([j, i, 1]))\n",
    "            p = p * depth_img[i, j]\n",
    "\n",
    "            # 将3D坐标添加到点云数组中\n",
    "            point_cloud[i * w + j, :] = p\n",
    "\n",
    "    return point_cloud\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Time elapsed:0.9333seconds \n"
     ]
    }
   ],
   "source": [
    "# 十次求平均\n",
    "# 相机内参矩阵\n",
    "k = np.array([[535.4, 0.0, 320.0],\n",
    "              [0.0, 539.2, 240.0],\n",
    "              [0.0, 0.0, 1.0]])\n",
    "\n",
    "elapsed_time_list = []\n",
    "for i in range(10):# \n",
    "    start_time = time.time()  # 记录开始时间\n",
    "\n",
    "    # 将深度图转换为点云\n",
    "    point_cloud = depth_to_point_cloud(depth_img, k)\n",
    "\n",
    "    end_time = time.time()  # 记录结束时间\n",
    "\n",
    "    elapsed_time = end_time - start_time  # 计算耗时\n",
    "\n",
    "    elapsed_time_list.append(elapsed_time)\n",
    "\n",
    "average_time = sum(elapsed_time_list) / len(elapsed_time_list)\n",
    "\n",
    "print(f\"Time elapsed average 10 times:{average_time:.4f} seconds.\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 可视化点云\n",
    "\n",
    "# 点云数据存储在名为point_cloud的numpy数组中\n",
    "cloud = o3d.geometry.PointCloud()\n",
    "cloud.points = o3d.utility.Vector3dVector(point_cloud)\n",
    "\n",
    "# 创建一个可视化窗口\n",
    "vis = o3d.visualization.Visualizer()\n",
    "vis.create_window()\n",
    "\n",
    "# 将点云添加到可视化窗口中\n",
    "vis.add_geometry(cloud)\n",
    "\n",
    "# 设置相机位置，没调所以可有可无\n",
    "vis.get_view_control().set_front([0, 0, -1])\n",
    "vis.get_view_control().set_lookat([0, 0, 0])\n",
    "vis.get_view_control().set_up([0, -1, 0])\n",
    "\n",
    "# 开始显示点云\n",
    "vis.run()\n",
    "\n",
    "# 关闭显示窗口\n",
    "vis.destroy_window()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "0.9s"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "env4cv",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
